﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Engine.Physics;
using Engine.Graphics;

namespace Engine
{
    static class GeometryUtils
    {
        const float EPA_TOLERANCE = 0.00001f;
        public static bool IsTriangleCCW(Vector2 A, Vector2 B, Vector2 C)
        {
            return ((A.X - C.X) * (B.Y - C.Y) - (B.X - C.X) * (A.Y - C.Y)) > 0 ? true : false;
        }

        public static float GetArea(List<Vector2> _points)
        {
            float area = 0;
            for (int i = 0; i < _points.Count; i++)
            {
                area += (_points[i].X - _points[(i + 1) % _points.Count].X) * (_points[i].Y + _points[(i + 1) % _points.Count].Y);
            }
            return area /= 2;
        }

        struct Edge
        {
            public Vector2 a;
            public Vector2 b;
            public Vector2 normal;
            public float distance;
            public int index;
        }

        public static bool GJKIntersect(CollisionShape a, CollisionShape b, ref CollisionInfo _info)
        {
            Vector2 s = Support(a, b, Vector2.UnitX);
            List<Vector2> simplex = new List<Vector2>(){s};
            Vector2 d = -Vector2.UnitX;
            int n = 1;
            _info.collding = false;
            do
            {
                Vector2 p = Support(a, b, d);
                if (Vector2.Dot(p, d) < 0) 
                    return false;
                simplex.Add(p);
                n = DoSimplex(ref simplex, ref d);
                _info.MTV = d;
            } while (n < 3);

            while (true)
            {
                Edge e = FindClosestEdge(simplex);
                Vector2 maxA, maxB;
                Vector2 p = Support(a, b, e.normal, out maxA, out maxB);

                float dist = Vector2.Dot(p, e.normal);
                if (dist - e.distance < EPA_TOLERANCE)
                {
                    _info.Normal = e.normal;
                    _info.MTV = e.distance * e.normal;
                    break;
                }
                else
                {
                    simplex.Insert(e.index, p);
                }
            }

            ClipShapes(a, b, ref _info);
            GraphicsEngine.Instance.DrawGizmo(_info.ContactPoints);
            _info.MTV *= -1;
            _info.Normal *= -1;
            _info.collding = true;
            return true;
        }

        private static void ClipShapes(CollisionShape _a, CollisionShape _b, ref CollisionInfo _info)
        {
            Vector2 maxA, e1A, e2A;
            _a.GetBestEdge(out maxA, out e1A, out e2A, _info.Normal);
            Vector2 maxB, e1B, e2B;
            _b.GetBestEdge(out maxB, out e1B, out e2B, -_info.Normal);

            Edge e1, e2;
            e1 = new Edge();
            e1.a = e1A;
            e1.b = e2A;

            e2 = new Edge();
            e2.a = e1B;
            e2.b = e2B;

            Edge refrence, incident;
            bool flip = false;

            Vector2 refMax;

            if (Vector2.Dot(e1.b - e1.a, _info.Normal) <= Vector2.Dot(e2.b - e2.a, _info.Normal))
            {
                refMax = maxA;
                refrence = e1;
                incident = e2;
            }
            else
            {
                refMax = maxB;
                refrence = e2;
                incident = e1;
                flip = true;
            }

            Vector2 refVec = refrence.b - refrence.a;
            refVec.Normalize();

            float o1 = Vector2.Dot(refVec, refrence.a);

            List<Vector2> clippedPoints = Clip(incident.a, incident.b, refVec, o1);
            if (clippedPoints.Count < 2)
                return;

            float o2 = Vector2.Dot(refVec, refrence.b);
            clippedPoints = Clip(clippedPoints[0], clippedPoints[1], -refVec, -o2);
            if (clippedPoints.Count < 2)
                return;

            Vector2 refNormal = MathUtils.Cross2D(-refVec * (refrence.b - refrence.a).Length());
            //if (!flip) refNormal = -refNormal;

            float max = Vector2.Dot(refNormal, refMax);

            int rmIndex = 1;
            if (Vector2.Dot(refNormal, clippedPoints[0]) - max < 0)
            {
                rmIndex--;
                clippedPoints.RemoveAt(0);
            }

            if (Vector2.Dot(refNormal, clippedPoints[rmIndex]) - max < 0)
                clippedPoints.RemoveAt(rmIndex);

            _info.ContactPoints = clippedPoints;
        }

        private static List<Vector2> Clip(Vector2 _a, Vector2 _b, Vector2 _n, float _o)
        {
            List<Vector2> clippedPoints = new List<Vector2>();

            float d1 = Vector2.Dot(_a, _n) - _o;
            float d2 = Vector2.Dot(_b, _n) - _o;

            if (d1 >= 0) clippedPoints.Add(_a);
            if (d2 >= 0) clippedPoints.Add(_b);

            if (d1 * d2 < 0)
            {
                Vector2 e = _b - _a;
                float u = d1 / (d1 - d2);
                e *= u;
                e += _a;
                clippedPoints.Add(e);
            }
            return clippedPoints;
        }

        private static Edge FindClosestEdge(List<Vector2> _simplex)
        {
            Edge closest = new Edge();
            // prime the distance of the edge to the max
            closest.distance = float.MaxValue;
            // s is the passed in simplex
            for (int i = 0; i < _simplex.Count; i++)
            {
                // compute the next points index
                int j = (i + 1 == _simplex.Count ? 0 : i + 1);
                // get the current point and the next one
                Vector2 a = _simplex[i];
                Vector2 b = _simplex[j];
                // create the edge vector
                Vector2 e = b - a;// or a.to(b);
                // get the vector from the origin to a
                Vector2 oa = a; // or a - ORIGIN
                // get the vector from the edge towards the origin
                Vector2 n = MathUtils.Cross2D(e);
                /*if (Vector2.Dot(e, oa) > 0)
                    n = -n;*/

                // normalize the vector
                n.Normalize();
                // calculate the distance from the origin to the edge
                float d = Vector2.Dot(n, a); // could use b or a here
                // check the distance against the other distances
                if (d < closest.distance)
                {
                    // if this edge is closer then use it
                    closest.a = a;
                    closest.b = b;
                    closest.distance = d;
                    closest.normal = n;
                    closest.index = j;
                }
            }
            // return the closest edge we found
            return closest;
        }

        private static int DoSimplex(ref List<Vector2> _simplex, ref Vector2 _dir)
        {
            if (_simplex.Count == 2)
                return DoSimplexLine(ref _simplex, ref _dir);
            else
                return DoSimplexTriangle(ref _simplex, ref _dir);
        }

        private static int DoSimplexLine(ref List<Vector2> _simplex, ref Vector2 _dir)
        {
            Vector2 a = _simplex[1];
            Vector2 b = _simplex[0];
            Vector2 ao = -a;
            Vector2 ab = b - a;

            _dir = MathUtils.Cross2D(ab);
            if (Vector2.Dot(ao, _dir) < 0)
                _dir = -_dir;

            return 2;
        }

        private static int DoSimplexTriangle(ref List<Vector2> _simplex, ref Vector2 _dir)
        {
            Vector2 a = _simplex[2];
            Vector2 b = _simplex[1];
            Vector2 c = _simplex[0];

            Vector2 ao = -a;
            Vector2 ab = b - a;
            Vector2 ac = c - a;

            _dir = MathUtils.Cross2D(ab);
            if (MathUtils.Cross2D(ab, ac) > 0)
                _dir = -_dir;

            if (0 < Vector2.Dot(ao, _dir))
            {
                _simplex.RemoveAt(0);
                return 2;
            }

            _dir = MathUtils.Cross2D(ac);
            if (MathUtils.Cross2D(ac, ab) > 0)
                _dir = -_dir;

            if (0 < Vector2.Dot(ao, _dir))
            {
                _simplex.RemoveAt(1);
                return 2;
            }

            return 3;
        }

        private static Vector2 Support(CollisionShape a, CollisionShape b, Vector2 _dir)
        {
            return a.GetMaxPoint(_dir) - b.GetMaxPoint(-_dir);
        }

        private static Vector2 Support(CollisionShape a, CollisionShape b, Vector2 _dir, out Vector2 _maxA, out Vector2 _maxB)
        {
            _maxA = a.GetMaxPoint(_dir);
            _maxB = b.GetMaxPoint(-_dir);
            return _maxA - _maxB;
        }
    }
}
